
// Copyright (c) 2012, 2013 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "openMVG/image/image.hpp"
#include "openMVG/image/image_warping.hpp"
#include "openMVG/features/features.hpp"
#include "openMVG/matching/regions_matcher.hpp"
#include "openMVG/multiview/solver_homography_kernel.hpp"
#include "openMVG/multiview/conditioning.hpp"
#include "openMVG/robust_estimation/robust_estimator_ACRansac.hpp"
#include "openMVG/robust_estimation/robust_estimator_ACRansacKernelAdaptator.hpp"

#include "nonFree/sift/SIFT_describer.hpp"

#include "third_party/stlplus3/filesystemSimplified/file_system.hpp"
#include "third_party/vectorGraphics/svgDrawer.hpp"

#include <string>
#include <iostream>

using namespace openMVG;
using namespace openMVG::image;
using namespace openMVG::matching;
using namespace openMVG::robust;
using namespace svg;
using namespace std;

int main() {

	Image<RGBColor> image;
	const string jpg_filenameL = stlplus::folder_up(string(THIS_SOURCE_DIR))
		+ "/imageData/StanfordMobileVisualSearch/Ace_0.png";
	const string jpg_filenameR = stlplus::folder_up(string(THIS_SOURCE_DIR))
		+ "/imageData/StanfordMobileVisualSearch/Ace_1.png";

	Image<unsigned char> imageL, imageR;
	ReadImage(jpg_filenameL.c_str(), &imageL);
	ReadImage(jpg_filenameR.c_str(), &imageR);

	//--
	// Detect regions thanks to an image_describer
	//--
	using namespace openMVG::features;
	std::unique_ptr<Image_describer> image_describer(new SIFT_Image_describer(SiftParams(-1)));
	std::map<IndexT, std::unique_ptr<features::Regions> > regions_perImage;
	image_describer->Describe(imageL, regions_perImage[0]);
	image_describer->Describe(imageR, regions_perImage[1]);

	const SIFT_Regions* regionsL = dynamic_cast<SIFT_Regions*>(regions_perImage.at(0).get());
	const SIFT_Regions* regionsR = dynamic_cast<SIFT_Regions*>(regions_perImage.at(1).get());

	const PointFeatures
		featsL = regions_perImage.at(0)->GetRegionsPositions(),
		featsR = regions_perImage.at(1)->GetRegionsPositions();

	// Show both images side by side
	{
		Image<unsigned char> concat;
		ConcatH(imageL, imageR, concat);
		string out_filename = "01_concat.jpg";
		WriteImage(out_filename.c_str(), concat);
	}

	//- Draw features on the two image (side by side)
  {
	  Image<unsigned char> concat;
	  ConcatH(imageL, imageR, concat);

	  //-- Draw features :
	  for (size_t i = 0; i < featsL.size(); ++i)  {
		  const SIOPointFeature point = regionsL->Features()[i];
		  DrawCircle(point.x(), point.y(), point.scale(), 255, &concat);
	  }
	  for (size_t i = 0; i < featsR.size(); ++i)  {
		  const SIOPointFeature point = regionsR->Features()[i];
		  DrawCircle(point.x() + imageL.Width(), point.y(), point.scale(), 255, &concat);
	  }
	  string out_filename = "02_features.jpg";
	  WriteImage(out_filename.c_str(), concat);
  }

  std::vector<IndMatch> vec_PutativeMatches;
  //-- Perform matching -> find Nearest neighbor, filtered with Distance ratio
  {
	  // Find corresponding points
	  matching::DistanceRatioMatch(
		  0.8, matching::BRUTE_FORCE_L2,
		  *regions_perImage.at(0).get(),
		  *regions_perImage.at(1).get(),
		  vec_PutativeMatches);

	  // Draw correspondences after Nearest Neighbor ratio filter
	  svgDrawer svgStream(imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height()));
	  svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height());
	  svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width());
	  for (size_t i = 0; i < vec_PutativeMatches.size(); ++i) {
		  //Get back linked feature, draw a circle and link them by a line
		  const SIOPointFeature L = regionsL->Features()[vec_PutativeMatches[i].i_];
		  const SIOPointFeature R = regionsR->Features()[vec_PutativeMatches[i].j_];
		  svgStream.drawLine(L.x(), L.y(), R.x() + imageL.Width(), R.y(), svgStyle().stroke("green", 2.0));
		  svgStream.drawCircle(L.x(), L.y(), L.scale(), svgStyle().stroke("yellow", 2.0));
		  svgStream.drawCircle(R.x() + imageL.Width(), R.y(), R.scale(), svgStyle().stroke("yellow", 2.0));
	  }
	  const std::string out_filename = "03_siftMatches.svg";
	  std::ofstream svgFile(out_filename.c_str());
	  svgFile << svgStream.closeSvgFile().str();
	  svgFile.close();
  }

  // Homography geometry filtering of putative matches
  {
	  //A. get back interest point and send it to the robust estimation framework
	  Mat xL(2, vec_PutativeMatches.size());
	  Mat xR(2, vec_PutativeMatches.size());

	  for (size_t k = 0; k < vec_PutativeMatches.size(); ++k)  {
		  const PointFeature & imaL = featsL[vec_PutativeMatches[k].i_];
		  const PointFeature & imaR = featsR[vec_PutativeMatches[k].j_];
		  xL.col(k) = imaL.coords().cast<double>();
		  xR.col(k) = imaR.coords().cast<double>();
	  }

	  //-- Homography robust estimation
	  std::vector<size_t> vec_inliers;
	  typedef ACKernelAdaptor<
		  openMVG::homography::kernel::FourPointSolver,
		  openMVG::homography::kernel::AsymmetricError,
		  UnnormalizerI,
		  Mat3>
		  KernelType;

	  KernelType kernel(
		  xL, imageL.Width(), imageL.Height(),
		  xR, imageR.Width(), imageR.Height(),
		  false); // configure as point to point error model.

	  Mat3 H;
	  const std::pair<double, double> ACRansacOut = ACRANSAC(kernel, vec_inliers, 1024, &H,
		  std::numeric_limits<double>::infinity(),
		  true);
	  const double & thresholdH = ACRansacOut.first;

	  // Check the homography support some point to be considered as valid
	  if (vec_inliers.size() > KernelType::MINIMUM_SAMPLES *2.5) {

		  std::cout << "\nFound a homography under the confidence threshold of: "
			  << thresholdH << " pixels\n\twith: " << vec_inliers.size() << " inliers"
			  << " from: " << vec_PutativeMatches.size()
			  << " putatives correspondences"
			  << std::endl;

		  //Show homography validated point and compute residuals
		  std::vector<double> vec_residuals(vec_inliers.size(), 0.0);
		  svgDrawer svgStream(imageL.Width() + imageR.Width(), max(imageL.Height(), imageR.Height()));
		  svgStream.drawImage(jpg_filenameL, imageL.Width(), imageL.Height());
		  svgStream.drawImage(jpg_filenameR, imageR.Width(), imageR.Height(), imageL.Width());
		  for (size_t i = 0; i < vec_inliers.size(); ++i)  {
			  const SIOPointFeature & LL = regionsL->Features()[vec_PutativeMatches[vec_inliers[i]].i_];
			  const SIOPointFeature & RR = regionsR->Features()[vec_PutativeMatches[vec_inliers[i]].j_];
			  const Vec2f L = LL.coords();
			  const Vec2f R = RR.coords();
			  svgStream.drawLine(L.x(), L.y(), R.x() + imageL.Width(), R.y(), svgStyle().stroke("green", 2.0));
			  svgStream.drawCircle(L.x(), L.y(), LL.scale(), svgStyle().stroke("yellow", 2.0));
			  svgStream.drawCircle(R.x() + imageL.Width(), R.y(), RR.scale(), svgStyle().stroke("yellow", 2.0));
			  // residual computation
			  vec_residuals[i] = std::sqrt(KernelType::ErrorT::Error(H,
				  LL.coords().cast<double>(),
				  RR.coords().cast<double>()));
		  }
		  string out_filename = "04_ACRansacHomography.svg";
		  ofstream svgFile(out_filename.c_str());
		  svgFile << svgStream.closeSvgFile().str();
		  svgFile.close();

		  // Display some statistics of reprojection errors
		  float dMin, dMax, dMean, dMedian;
		  minMaxMeanMedian<float>(vec_residuals.begin(), vec_residuals.end(),
			  dMin, dMax, dMean, dMedian);

		  std::cout << std::endl
			  << "Homography matrix estimation, residuals statistics:" << "\n"
			  << "\t-- Residual min:\t" << dMin << std::endl
			  << "\t-- Residual median:\t" << dMedian << std::endl
			  << "\t-- Residual max:\t " << dMax << std::endl
			  << "\t-- Residual mean:\t " << dMean << std::endl;


		  //---------------------------------------
		  // Warp the images to fit the reference view
		  //---------------------------------------
		  // reread right image that will be warped to fit left image
		  ReadImage(jpg_filenameR.c_str(), &image);
		  WriteImage("query.png", image);

		  // Create and fill the output image
		  Image<RGBColor> imaOut(imageL.Width(), imageL.Height());
		  image::Warp(image, H, imaOut);
		  const std::string imageNameOut = "query_warped.png";
		  WriteImage(imageNameOut.c_str(), imaOut);
	  }
	  else  {
		  std::cout << "ACRANSAC was unable to estimate a rigid homography"
			  << std::endl;
	  }
  }
  return EXIT_SUCCESS;
}

